#include   "RoboDrive.h"
#include "vex.h"
#include "RoboDevice.h"
using namespace vex;
namespace RoboDrive{

    double R  = 0.041275;
    double Pi = acos(-1);
    double Gamma = 1.1850;
    double D_wb  = 0.279;//车子的两个轮子中心的相距距离
    double D_lb = 0.204;//同侧前后轮的相距距离

    //motor


    /**
     * @brief 左轮旋转
     * 
     * @param V 旋转速度，单位：m/s
     */
    void Vl(double V){
        double W = V / R;
        RoboDevice::LeftFrontMotor.spin(fwd,W*360/(2*Pi), dps);
        RoboDevice::LeftBackMotor.spin(fwd,W*360/(2*Pi), dps);
    }
    
    /**
     * @brief 右轮旋转
     * 
     * @param V 旋转速度，单位：m/s
     */
    void Vr(double V){//右边两轮以V m/s 转
        double W = V / R;
        RoboDevice::RightFrontMotor.spin(fwd,W*360/(2*Pi),dps);
        RoboDevice::RightBackMotor.spin(fwd,W*360/(2*Pi),dps);
    }

    void Vr_l(double V){//右边两轮以V m/s 转
        double W = V / R;
        RoboDevice::RightFrontMotor.spin(fwd,W*360/(2*Pi),dps);
        RoboDevice::LeftFrontMotor.spin(fwd,W*360/(2*Pi), dps);
        RoboDevice::LeftBackMotor.spin(fwd,W*360/(2*Pi), dps);
        RoboDevice::RightBackMotor.spin(fwd,W*360/(2*Pi),dps);
    }
    
    /**
     * @brief 四轮同时制动
     * 
     */
    void BrakeMotor(){

        RoboDevice::RightBackMotor.stop(brake);   
        RoboDevice::LeftBackMotor.stop(brake);
        RoboDevice::LeftFrontMotor.stop(brake);
        RoboDevice::RightFrontMotor.stop(brake);
         
    }//四轮同时制动,急刹，可能会使电机过热，后轮刹车，防止前倾

    void BrakeMotor_1(){

        RoboDevice::RightBackMotor.stop(coast);   
        RoboDevice::LeftBackMotor.stop(coast);
        // RoboDevice::LeftFrontMotor.stop(coast);
        // RoboDevice::RightFrontMotor.stop(coast);
        
         
    }//不再通电

    /**
     * @brief 前进
     * 
     * @param v 前进速度，单位：m/s
     * @param s 前进距离，单位：m(如果没有设定距离则一直前进)
     */
    void MoveForward(double v, double s ){                //以 V m/s 的速度 向前移动 S m  ,  （向前直线）
    double Curtime = RoboDevice::SystemTimer.time(sec);//测试用
    double Curtime_1 = RoboDevice::SystemTimer.time(sec);//测试用
    double Time = s/v;
    double k = 0;
        if (s!=1000){
            while (Curtime_1<= Time + Curtime)
            {
                Curtime_1 = RoboDevice::SystemTimer.time(sec);
                k = 2*(Curtime_1-Curtime);
                if (k>=1) k = 1;
                
                // printf("%.6f\n",Curtime);
                Vr_l(k*3*v/5);
                // Curtime = RoboDevice::SystemTimer.time(sec);
                // printf("%.6f\n",Curtime);
                wait(0.02,sec);
            }
            BrakeMotor_1();
            
        }
        else{
            Vr(3*v/5);Vl(3*v/5);
        }
        
    }

    /**
     * @brief 后退
     * 
     * @param v 后退速度，单位：m/s
     * @param s 后退距离，单位：m(如果没有设定距离则一直后退)
     */
    void MoveBackward(double v,double s ){               //以 V m/s 的速度 向后移动 S m  ,  （向后直线）
        if (s!=1000)
        {
        double Time = s/v;
        Vr(-v*3/5);Vl(-v*3/5); 
                wait(Time,sec);
        BrakeMotor();
        }else{
            Vr(-v*3/5);Vl(-v*3/5);
        }
    
    }

   
    /**
     * @brief 以 V m/s 的速度 ，以 W 的角速度 向左前移动  （-V ，W 为右后；    V ，-W为右前；  -V ,-W 为左后）
     * 
     * @param Vc 车子的纵向速度，单位：m/s
     * @param Wc 车子的旋转速度，弧度制
     */
    void Drive(double Vc,double Wc){               
        double VR = Vc+Gamma*D_wb*Wc/2;
        double VL = Vc-Gamma*D_wb*Wc/2;
        Vl(VL/2);
        Vr(VR/2);
        
    }

    /**
     * @brief Get the Wc 获取角速度
     * 
     * @return double 返回角速度，为弧度制
     */
    double GetWc(){                                     
        double Wc = 0;
        Wc = -RoboDevice::Inertial.gyroRate(zaxis, dps);
        Wc = Pi*Wc/180.0;
        //*Pi/180
        return Wc;
    }

    /**
     * @brief 初始化传感器
     * 
     */
    

    /**
     * @brief 关于RoboDrive的PID算法升级，更加稳定可靠
     * 
     * @param Vc 车子的预期纵向速度，单位：m/s
     * @param Wc  车子的预期旋转速度，弧度制,正数为逆时针旋转
     * @param time 预期的完成时间,单位：s
     */
    void RoboDrivePID(double Vc,double Wc,double time){ //PID
        // double Ti = 10;
        double Kp = 0.0001;
        // double Td = 0.01;
        double T = 0.01;
        double Ki = 0.0003;
        double Kd = 0.25;
        double ek = 0, ek_1 = 0, ek_2 = 0;
        double Wc_u = Wc;
        double T_sum = 0;
        double Max = 2.7*Pi; 
        // double Min = 0;

        if (Vc == 0)
        {
            // RoboDevice::RoboDeviceInit();
        }
        

        while(T_sum<=time){

            Drive(Vc,Wc_u);
            
            ek_2 = ek_1;
            ek_1 = ek;
            ek = Wc - GetWc();//涉及用到测量角速度的传感器
            

            Wc_u+=Kp*(ek - ek_1)+Ki*ek + Kd*(ek - 2*ek_1+ek_2);//无传感器输出当
            
            //前角速度，Wc-u不断被积分，数值不断变大，会造成不小的误差

            if(Wc_u > Max){ 
			          Wc_u = Max;
            }
            // if(Wc_u < Min){
            //     Wc_u = Min;
            // }
            // printf("%.3f,%.3f\n",GetWc(),Wc_u);//PID调试函数
            wait(T,sec);
            T_sum+=T;
        }
        
            BrakeMotor();
        
    }

    /**
     * @brief //自转Angle （度）的角度  【+为逆时针转 ； -为顺时针转】
     * 
     * @param Angle 自旋的角度,角度值,正数为逆时针旋转
     * @param time 设定的完成时间，单位：s
     */
    void Rotate(double Angle,double time){                          
        double Wc = (Angle/180*Pi)/time;
        RoboDrivePID(0, Wc, time); //更改前的wait被移至RoboDrivePID中
        BrakeMotor();
    }

    /**
     * @brief 以 R（m）为半径，绕心转 angle（度）角度 【+为左转 ； -为右转】
     * 
     * @param Rc 绕心旋转的轨迹圆周半径，单位：m,正数为后退
     * @param angle 绕心旋转的角度，为角度制，正数为逆时针
     * @param FinishTime 程序完成的时间
     * (--:左后，-+:右后，+-:右前,++：左前)
     */
    //强化目的：消除输入的角度与最后目标角度之间的差值，采用位置型PI算法对每个采样周期内的已旋转弧度进行计算并在下一个周期内进行补偿
    void MovewithAngle(double Rc,double angle,double FinishTime){                 
        // double FinishTime = 2.0;//默认在三秒内完成
        double v =(Rc*2*Pi*(fabs(angle)/360))/FinishTime;
        double PresentAngle;//已旋转的弧度
        double ExpectingAngle;//期望已旋转的弧度
        double eAngle_1 = 0;//转动的弧度误差
        double eAngle_2 = 0;
        double Kp = 1.3;
        double Ki = 0.6;
        double Tc = 0.05;//采样周期
        double w = (Pi*angle/180)/FinishTime;//串级外环需要求得期望的角速度，通过运行的轨迹解算，弧度制
        double w_u = w;
        double LeftFrontMotorPosition;
        double LeftBackMotorPosition;
        double RightFrontMotorPosition;
        double RightBackMotorPosition;//各个轮子的旋转弧度,
        double RC = sqrt((Rc-D_wb/2)*(Rc-D_wb/2)+(D_lb/2)*(D_lb/2));
        double RF = sqrt((Rc-D_wb/2)*(Rc-D_wb/2)+(D_lb/2)*(D_lb/2));
        double Sytime = RoboDevice::SystemTimer.time(sec);
        double Curtime = RoboDevice::SystemTimer.time(sec);
        //RoboDevice::RoboDeviceInit();

        // printf("Beginingtime:%.3f\n",Sytime);

        RoboDevice::LeftFrontMotor.resetPosition();
        RoboDevice::LeftBackMotor.resetPosition();
        RoboDevice::RightFrontMotor.resetPosition();
        RoboDevice::RightBackMotor.resetPosition();

        while(Curtime<=Sytime+FinishTime){

            LeftFrontMotorPosition = Pi*RoboDevice::LeftFrontMotor.position(deg)/180;
            LeftBackMotorPosition = Pi*RoboDevice::LeftBackMotor.position(deg)/180;
            RightFrontMotorPosition = Pi*RoboDevice::RightFrontMotor.position(deg)/180;
            RightBackMotorPosition = Pi*RoboDevice::RightBackMotor.position(deg)/180;//从电机获取已旋转的弧度

            PresentAngle = ((5*R*LeftFrontMotorPosition/3)/RC+(5*R*LeftBackMotorPosition/3)/RC+(5*R*RightFrontMotorPosition/3)/RF+(5*R*RightBackMotorPosition/3)/RF)/4;//四轮弧度求平均
            ExpectingAngle = (Curtime-Sytime) * w ;

            eAngle_2 = eAngle_1;
            eAngle_1 = ExpectingAngle - PresentAngle;

            // printf("%.3f  %.3f\n",ExpectingAngle,PresentAngle);
            // printf("time: %.3f\n",Curtime);//PI调试函数

            w_u = Kp*(w+(eAngle_1+eAngle_2)*Ki);


            RoboDrivePID(v,w_u,Tc);//补偿旋转的弧度差
            
            
            Curtime = RoboDevice::SystemTimer.time(sec);;

        }

        BrakeMotor_1();

    }
    
    /**
     * @brief Get the Distance //通过激光传感器获取距离目标的距离
     * 
     * @return double 返回距离，单位未确定
     */
    double GetDistance(){                                                  //获取距离目标的距离
        double Distance = 0;
        return Distance;
    }

    /**
     * @brief 根据激光传感器获取的数值进行的PID增量型移动算法，保持速度的稳定
     * 
     * @param v 前进速度，单位：m/s
     */
    void MoveWithDM(double v){
        double v_p = v;
        double Ti = 10000000000;//积分时间常数
        double Td = 1;//微分时间常数
        double T = 0.01;//采样周期，程序循环一次的
        double Kp = 1;
        double Ki = Kp/Ti;
        double Kd = (Kp*Td)/T;

        double d_1  = 0,d_2 = 0;
        double v_1 = 0,v_2 = 0;

        d_1 = GetDistance();//获取初始距离
        while(true){

            MoveForward(v_p);

            d_2 = GetDistance();//时间T后的相距距离

            v_2 = v_1;
            v_1 = v;
            v_p= (d_1-d_2)/T-v;//速度的偏差量

            v_p += Kp*(v-v_1)+Ki*v+Kd*(v-2*v_1+v_2);//通过PID进行增量计算
            d_1 = GetDistance();
            wait(T,sec);

        }
    }

    void MoveParallelly(double v){

    double DistanceForward = 0.3;
    double tan_1 ;
    double angle ;
    double Distance_1,Distance_2;
    Distance_1 = RoboDevice::RoboDistance.objectDistance(mm)/1000;
    Distance_2 = RoboDevice::RoboDistance.objectDistance(mm)/1000;
    wait(20,msec);

    do{
        Distance_1 = RoboDevice::RoboDistance.objectDistance(mm)/1000;
        MoveForward(v,DistanceForward);
        Distance_2 = RoboDevice::RoboDistance.objectDistance(mm)/1000;
        tan_1 = (Distance_1-Distance_2)/DistanceForward;
        printf("%.3f %.3f \n",RoboDevice::RoboDistance.objectDistance(mm)/1000,RoboDevice::RoboDistance.objectDistance(mm)/1000);
        angle = 90-180*atan(tan_1)/Pi;
        printf("%.3f\n",angle);
        Drive(0.3,angle*Pi/180*5);
        Distance_1 = RoboDevice::RoboDistance.objectDistance(mm)/1000;
        Distance_2 = RoboDevice::RoboDistance.objectDistance(mm)/1000;
        wait(20,msec);
    }while(fabs(Distance_1-Distance_2)>=0.08);
    
    MoveForward(v,0.5);
    }
    
}


